#include <iostream>
#include <camera/camera_wrapper.h>
#include <additions.h>
#include <options.h>
#include "options.h"
#include "log.h"

using namespace std;
using namespace cv;
/*!
 *
 * @param exposure
 * @param gain
 * @param camera_mode
 * @param n
 */
CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std::string &n) :
        name(n),
        init_done(false),
        mode(camera_mode),
        camera_cnts(2),
        camera_status(-1),
        iplImage(nullptr),
        rgb_buffer(nullptr),
        channel(3),
        gain(gain),
        exposure_time_ms(exposure){
}

//void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext){
//    CameraWrapper *c = (CameraWrapper*)pContext;
//    CameraImageProcess(hCamera, pFrameBuffer, c->rgb_buffer, pFrameHead);
//    auto iplImage = cvCreateImageHeader(cvSize(pFrameHead->iWidth, pFrameHead->iHeight), IPL_DEPTH_8U, c->channel);
//    cvSetData(iplImage, c->rgb_buffer, pFrameHead->iWidth * c->channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
//    c->src_queue.push(cv::cvarrToMat(iplImage).clone());
//}

bool CameraWrapper::init() {
    CameraSdkInit(1);//1表示选择SDK内部提示信息为中文
    int camera_enumerate_device_status = CameraEnumerateDevice(camera_enum_list, &camera_cnts);//// 在调用CameraInit之前，必须调用该函数来获得设备的信息，枚举设备，并建立设备列表。成功返回0。
    if (camera_enumerate_device_status != CAMERA_STATUS_SUCCESS) {
        LOGE("CameraEnumerateDevice fail with %d!",camera_enumerate_device_status);
    }
    if (camera_cnts == 0) {
        LOGE("No camera device detected!");
        return false;
    } else if (camera_cnts >= 1) {
        LOGS("%d camera device detected!",camera_cnts);
    }
    int i;
    for (i = 0; i < camera_cnts; i++) {
        camera_status = CameraInit(&camera_enum_list[i], -1, -1, &h_camera);////相机初始化。h_camera是代表相机身份的句柄；在调用其他相机相关的操作时，都需要传入该句柄。
        if (camera_status != CAMERA_STATUS_SUCCESS) {
            LOGE("CameraInit return error code %d", camera_status);
            CameraUnInit(h_camera);
            continue;
        }
    }
//    if (i >= camera_cnts) {
//        cerr<<"No device name %s or device open error!!"<<endl;
//        return false;
//    }

    auto status = CameraGetCapability(h_camera, &tCapability);//返回相机特性描述：tSdkCameraCapbility， pCameraInfo 指针，返回该相机特性描述的结构体。
    if (status != CAMERA_STATUS_SUCCESS) {
        LOGE("CameraGetCapability return error code %d!",status);
        return false;
    }

    rgb_buffer = (unsigned char *) malloc(tCapability.sResolutionRange.iHeightMax *
                                          tCapability.sResolutionRange.iWidthMax * 3);//3表示三通道

    //CameraReadParameterFromFile(h_camera, PROJECT_DIR"/others/MV-UB31-Group0.config");//从PC上指定的参数文件中加载参数。我公司相机参数保存在PC上为.config后缀的文件，位于安装下的Camera\Configs文件夹中。
    CameraLoadParameter(h_camera, PARAMETER_TEAM_A);//加载指定组的参数到相机中。
    CameraSetAeState(h_camera, false);//设置相机自动曝光或手动曝光，FALSE为手动。
    CameraSetExposureTime(h_camera, exposure_time_ms * 1000);
    CameraSetFrameSpeed(h_camera, 1);//设定相机输出图像的帧率,0对应于低速模式，1对应于普通模式，2对应于高速模式,最多只能到1.
    CameraSetAnalogGain(h_camera, gain);//设置相机的图像模拟增益值。

//#if ENEMY_COLOR==ENEMY_BLUE
//    CameraSetGain(h_camera,70,100,200);//实际的放大倍数是设定值/100。RGB
//#else
//    CameraSetGain(h_camera,70,100,200);
//#endif

    double t;
    int g;
    CameraGetExposureTime(h_camera, &t);
    CameraGetAnalogGain(h_camera, &g);
    LOGM("Exposure time: %lfms, gain:%d", t / 1000.0, g);

//    char **buf;
//    CameraGetInformation(h_camera,buf);
//    std::cout<<"CAMERA_INFO:%s"<<*buf<<std::endl;
    /*让SDK进入工作模式，开始接收来自相机发送的图像
    数据。如果当前相机是触发模式，则需要接收到
    触发帧以后才会更新图像。    */
    CameraPlay(h_camera);

    /*其他的相机参数设置
    例如 CameraSetExposureTime   CameraGetExposureTime  设置/读取曝光时间
         CameraSetImageResolution  CameraGetImageResolution 设置/读取分辨率
         CameraSetGamma、CameraSetContrast、CameraSetGain等设置图像伽马、对比度、RGB数字增益等等。
         CameraGetFriendlyName    CameraSetFriendlyName 获取/设置相机名称（该名称可写入相机硬件）
    */

    if (tCapability.sIspCapacity.bMonoSensor) {
        channel = 1;
        CameraSetIspOutFormat(h_camera, CAMERA_MEDIA_TYPE_MONO8);
        //LOGM("camera %s mono ", camera_name);
    } else {
        channel = 3;
        CameraSetIspOutFormat(h_camera, CAMERA_MEDIA_TYPE_BGR8);
        //LOGM("camera %s color ", camera_name);
    }
//    if(mode == 2){
//        CameraSetCallbackFunction(h_camera, cameraCallback, this, nullptr);
//    }
    init_done = true;
    return true;
}

void CameraWrapper::SetResolution(cv::Size roi, cv::Size offset )
{
    tImageResolution.iIndex = 0xff;//iIndex：索引号；OXFF 表示自定义分辨率(ROI)
    tImageResolution.iWidth = roi.width;// 相机最终输出的图像的宽度
    tImageResolution.iHeight = roi.height;// 相机最终输出的图像的高度
    tImageResolution.iWidthFOV = roi.width;//采集视场的宽度
    tImageResolution.iHeightFOV = roi.height;//
    tImageResolution.iHOffsetFOV = ((1280 - roi.width) / 2) + offset.width; //采集视场相对于Sensor最大视场左上角的垂直偏移
    tImageResolution.iVOffsetFOV = ((1024 - roi.height) / 2) + offset.height;/// 采集视场相对于Sensor最大视场左上角的水平偏移
    CameraSetMirror(h_camera,1,true);//设置图像镜像操作。镜像操作分为水平和垂直两个方向。0水平，1垂直。
    CameraSetMirror(h_camera,0,true);
    CameraSetImageResolution(h_camera, &tImageResolution);//设置预览的分辨率。
}

bool CameraWrapper::read(cv::Mat &src) {
    if(init_done) {
        if (mode == 0)return readProcessed(src);
        if (mode == 1)return readRaw(src);
        //if (mode == 2)return readCallback(src);
    } else {
        return false;
    }
}

bool CameraWrapper::readRaw(cv::Mat &src) {
    if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
        if (iplImage) {
            cvReleaseImageHeader(&iplImage);
        }

        iplImage = cvCreateImageHeader(Size(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, 1);

        cvSetData(iplImage, pby_buffer, frame_info.iWidth);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率

        src = cvarrToMat(iplImage).clone();

        //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
        //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
        CameraReleaseImageBuffer(h_camera, pby_buffer);

        return true;
    } else {
        src = cv::Mat();
        return false;
    }
}

bool CameraWrapper::readProcessed(cv::Mat &src) {
//	cerr << "Get-1" << endl;
    if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
        CameraImageProcess(h_camera, pby_buffer, rgb_buffer,
                           &frame_info);  // this function is super slow, better not to use it.
        //将获得的相机原始输出图像数据进行处理，叠加饱和度、颜色增益和校正、降噪等处理效果，最后得到RGB888格式的图像数据。
        if (iplImage) {
            cvReleaseImageHeader(&iplImage);
        }
        iplImage = cvCreateImageHeader(Size(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, channel);
        cvSetData(iplImage, rgb_buffer, frame_info.iWidth * channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
        src = cvarrToMat(iplImage).clone();
        //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
        //否则再次调用CameraGetImageBuffer        时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
        CameraReleaseImageBuffer(h_camera, pby_buffer);
        return true;
    } else {
        LOGE("ReadFromCamera return error code %d",read_status);
        src = cv::Mat();
        return false;
    }
}

//bool CameraWrapper::readCallback(cv::Mat &src) {
//    systime ts, te;
//    getsystime(ts);
//    while(src_queue.empty()){
//        getsystime(te);
//        if(getTimeIntervalms(te, ts) > 500){
//            return false;
//        }
//    }
//    return src_queue.pop(src);
//}

CameraWrapper::~CameraWrapper() {
    CameraUnInit(h_camera);
    //注意，先反初始化后再free
    if (rgb_buffer != nullptr)
        free(rgb_buffer);
}

bool checkReconnect(bool is_camera_connect,WrapperHead *&video) {
    if (!is_camera_connect) {
        LOGW("Camera offline");
        int curr_gain = ((CameraWrapper*)video)->gain;
        int curr_exposure = ((CameraWrapper*)video)->exposure_time_ms;
        delete video;
        video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
        is_camera_connect = video->init();
    }
    return is_camera_connect;
}
